//
// Created by bourne on 2024/4/23.
//
#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>
#include <string>
#include <Eigen/Eigen>
#include <ros/ros.h>
#include <nav_msgs/Path.h>
#include <ros/package.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Point.h>
#include <iomanip>

#include <bspline/Bspline.h>
#include <bspline/non_uniform_bspline.h>

using namespace std;
using namespace fast_planner;

ros::Publisher cpts_pub_;
ros::Publisher waypts_pub_;
ros::Publisher traj_pub_;

ros::Publisher bspline_pub_;

vector<Eigen::Vector3d> waypts_;
vector<double> yaw_waypts_;

Eigen::MatrixXd ctrl_pts_;
vector<double> yaw_control_pts;

double init_knot_span;
double yaw_dt;
Eigen::VectorXd knot_vector;

nav_msgs::Path ros_way_points_;
nav_msgs::Path ros_cpts_;
nav_msgs::Path ros_traj_;

NonUniformBspline traj;
NonUniformBspline yaw_traj;
vector<NonUniformBspline> derivative;
vector<NonUniformBspline> yaw_derivative;

bool have_cpts_;
bool have_waypts_;
bool have_traj_;
bool have_bspline_;

double vel = 1.0; // vel to cal knot vector

Eigen::Vector3d vel_start = Eigen::Vector3d (0.2,0.0,0.0);
Eigen::Vector3d acc_start = Eigen::Vector3d (0.0,0.0,0.0);
double yaw_rate_start = 0.0;
double yaw_acc_start = 0.0;

Eigen::Vector3d vel_end = Eigen::Vector3d (0.0,0.0,0.0);
Eigen::Vector3d acc_end = Eigen::Vector3d (0.0,0.0,0.0);
double yaw_rate_end = 0.0;
double yaw_acc_end = 0.0;

int degree = 3; // bspline degree

double max_v = 0.5; // vel boundary
double max_a = 1.0; // acc boundary
double max_yaw_rate = 1.0; // yaw rate boundary
double max_yaw_acc_rate = 1.0; // yaw acc boundary

bspline::Bspline bspline_;

void init(){
  have_cpts_ = false;
  have_waypts_ = false;
  have_traj_ = false;
  have_bspline_ = false;

  ROS_WARN_STREAM("params init done!");
}


void load_init_path()
{
  // load the path from the data
  std::string PathFileName = ros::package::getPath("test_bspline") + "/data/path.txt";
  std::ifstream infile(PathFileName);
  vector<vector<string>> data;
  while (infile)
  {
    string s;
    if (!getline(infile, s)) break;

    istringstream ss(s);
    vector <string> record;

    while (ss)
    {
      string s;
      if (!getline(ss, s, ',')) break;
      record.push_back(s);
    }

    geometry_msgs::PoseStamped path_point;
    path_point.pose.position.x = stod(record[0]);
    path_point.pose.position.y = stod(record[1]);
    path_point.pose.position.z = stod(record[2]);

    path_point.pose.orientation.x = 0;
    path_point.pose.orientation.y = 0;
    path_point.pose.orientation.z = 0;
    path_point.pose.orientation.w = 1;

    ros_way_points_.poses.push_back(path_point);
  }

  ros_way_points_.header.frame_id = "map";

  std::cout<<"The waypoint is "<<std::endl;
  Eigen::Vector3d waypoint;
  for(auto & pose : ros_way_points_.poses){
    waypoint[0] = pose.pose.position.x;
    waypoint[1] = pose.pose.position.y;
    waypoint[2] = pose.pose.position.z;
    std::cout<<waypoint.transpose()<<" | ";
    waypts_.push_back(waypoint);
  }
  std::cout<<std::endl;
  ROS_WARN_STREAM("The waypoint num is : "<<waypts_.size());

  have_waypts_ = true;
  ROS_WARN_STREAM("get waypoint done");
}



void get_traj(){
  // get control pts
  double length = -100.0;
  for(int i = 0; i<(int)waypts_.size()-1;i++){
    double tmp_length = (waypts_[i+1] - waypts_[i]).norm();
    if(tmp_length > length){
      length = tmp_length;
    }
  }
  std::cout<<"the max length interval is : "<<length<<std::endl;

  init_knot_span = length/vel;
  std::cout<<"the init knot snap is : "<<init_knot_span<<std::endl;

  vector<Eigen::Vector3d> start_end_derivative;
  start_end_derivative.push_back(vel_start);
  start_end_derivative.push_back(vel_end);
  start_end_derivative.push_back(acc_start);
  start_end_derivative.push_back(acc_end);

  std::cout<<"before parameterize"<<std::endl;
  NonUniformBspline::parameterizeToBspline(init_knot_span,waypts_,start_end_derivative,degree,ctrl_pts_);

  std::cout<<"The control pts size is "<<ctrl_pts_.rows()<<std::endl;
  std::cout<<ctrl_pts_<<std::endl;
  std::cout<<"==============="<<std::endl;

  ROS_WARN_STREAM("get control points done!");

  // pub control pts
  geometry_msgs::PoseStamped ctrl_pt;
  for(int i=0; i<(int)ctrl_pts_.rows(); i++){
    ctrl_pt.pose.position.x = ctrl_pts_(i,0);
    ctrl_pt.pose.position.y = ctrl_pts_(i,1);
    ctrl_pt.pose.position.z = ctrl_pts_(i,2);

    ctrl_pt.pose.orientation.x = 0;
    ctrl_pt.pose.orientation.y = 0;
    ctrl_pt.pose.orientation.z = 0;
    ctrl_pt.pose.orientation.w = 1;

    ros_cpts_.poses.push_back(ctrl_pt);
  }
  ros_cpts_.header.frame_id = "map";
  have_cpts_ = true;

  ROS_WARN_STREAM("pub control points done!");

  // set bspline
  traj.setUniformBspline(ctrl_pts_,degree,init_knot_span);

  std::cout<<"the knot vector is : "<<std::endl;
//  traj.printKnotVector();
  knot_vector = traj.getKnot();
  ROS_WARN_STREAM("The knot vector size is : " << knot_vector.rows());
  double duration = traj.getTimeSum();
  std::cout<<"the duration is : "<<duration<<std::endl;

  ROS_WARN_STREAM("get b-spline done!");

  // pub bspline
  double time = 0.0;
  geometry_msgs::PoseStamped traj_pt;
  while (time < duration + 1e-4){
    Eigen::Vector3d pos = traj.evaluateDeBoorT(time);
    traj_pt.pose.position.x = pos(0);
    traj_pt.pose.position.y = pos(1);
    traj_pt.pose.position.z = pos(2);

    traj_pt.pose.orientation.x = 0;
    traj_pt.pose.orientation.y = 0;
    traj_pt.pose.orientation.z = 0;
    traj_pt.pose.orientation.w = 1;

    ros_traj_.poses.push_back(traj_pt);

    time = time + 0.1;
  }

  ros_traj_.header.frame_id = "map";
  have_traj_ = true;

  ROS_WARN_STREAM("pub bspline done!");

  // get derivative for the bspline
  traj.computeDerivatives(2,derivative);

  ROS_WARN_STREAM("compute derivative done!");
  ROS_WARN_STREAM("get traj done!");
}

void displayCallback(const ros::TimerEvent&event){
  if (have_waypts_)
    waypts_pub_.publish(ros_way_points_);
  if (have_cpts_)
    cpts_pub_.publish(ros_cpts_);
  if (have_traj_)
    traj_pub_.publish(ros_traj_);
  if (have_bspline_){
    bspline_pub_.publish(bspline_);
  }
}


void printPathAndSave(){

  std::string PathFileName = ros::package::getPath("test_bspline") + "/data/t_p_v_a.txt";
  std::fstream f;
  f.open(PathFileName,std::ios::out);


  double duration = traj.getTimeSum();
  std::cout<<"the duration is : "<<duration<<std::endl;
  double time = 0.0;

  while (time < duration + 1e-4){
    Eigen::Vector3d pos = traj.evaluateDeBoorT(time);
    Eigen::Vector3d vel = derivative[0].evaluateDeBoorT(time);
    Eigen::Vector3d acc = derivative[1].evaluateDeBoorT(time);
    //std::cout<<"t: "<<time<<"|pos: "<<pos.transpose()<<" |vel: "<<vel.transpose()<<" |acc: "<<acc.transpose()<<std::endl;

    f<<time<<","
     <<pos(0)<<","<<pos(1)<<","<<pos(2)<<","
     <<vel(0)<<","<<vel(1)<<","<<vel(2)<<","
     <<acc(0)<<","<<acc(1)<<","<<acc(2)<<"\n";

    time = time + 0.1;
  }

  f.close();
  ROS_WARN_STREAM("Print and save done.");
}

void reallocate(){
  traj.setPhysicalLimits(max_v,max_a);
  bool fea = traj.checkFeasibility(false);
  int iter_num = 0;

  while (!fea && ros::ok()) {
    fea = traj.reallocateTime();
    if (++iter_num >= 3) break;
  }
  traj.computeDerivatives(2,derivative);
}

void printPathAndSaveAfterReallocate(){

  std::string PathFileName = ros::package::getPath("test_bspline") + "/data/t_p_v_a_re.txt";
  std::fstream f;
  f.open(PathFileName,std::ios::out);


  double duration = traj.getTimeSum();
  std::cout<<"the duration is : "<<duration<<std::endl;
  double time = 0.0;

  while (time < duration + 1e-4){
    Eigen::Vector3d pos = traj.evaluateDeBoorT(time);
    Eigen::Vector3d vel = derivative[0].evaluateDeBoorT(time);
    Eigen::Vector3d acc = derivative[1].evaluateDeBoorT(time);
    //std::cout<<"t: "<<time<<"|pos: "<<pos.transpose()<<" |vel: "<<vel.transpose()<<" |acc: "<<acc.transpose()<<std::endl;

    f<<time<<","
     <<pos(0)<<","<<pos(1)<<","<<pos(2)<<","
     <<vel(0)<<","<<vel(1)<<","<<vel(2)<<","
     <<acc(0)<<","<<acc(1)<<","<<acc(2)<<"\n";

    time = time + 0.1;
  }

  f.close();
  ROS_WARN_STREAM("Print and save re done.");
}


void calYaw(){
  ROS_WARN_STREAM("Cal Yaw");
  ROS_WARN_STREAM("The first pt is : " << waypts_[0].transpose());
  ROS_WARN_STREAM("The second pt is : "<< waypts_[1].transpose());

  // cal the frist yaw to test
  double first_yaw = atan2(waypts_[1].y() - waypts_[0].y(), waypts_[1].x() - waypts_[0].x());
  ROS_WARN_STREAM("The first yaw is : " << first_yaw);

  // cal yaw waypoints
  for(int i = 0; i < (int)waypts_.size() - 1; i++){
    double yaw_wpt = atan2(waypts_[i+1].y() - waypts_[i].y(), waypts_[i+1].x() - waypts_[i].x());
    yaw_waypts_.push_back(yaw_wpt);
  }
  yaw_waypts_.push_back(yaw_waypts_.back());
  ROS_WARN_STREAM("The yaw waypts size is : " << yaw_waypts_.size());

  // param to bspline
  vector<Eigen::Vector3d> yaw3dwpts;
  for(int i = 0; i < (int)yaw_waypts_.size(); i++){
    Eigen::Vector3d yaw3dwpt = Eigen::Vector3d (yaw_waypts_[i], 0.0, 0.0);
    yaw3dwpts.push_back(yaw3dwpt);
  }
  ROS_WARN_STREAM("The yaw 3d pts size is : "<<yaw3dwpts.size());

  Eigen::Vector3d yaw3d_rate_start = Eigen::Vector3d (yaw_rate_start, 0.0, 0.0);
  Eigen::Vector3d yaw3d_acc_start = Eigen::Vector3d (yaw_acc_start, 0.0, 0.0);
  Eigen::Vector3d yaw3d_rate_end = Eigen::Vector3d (yaw_rate_end, 0.0, 0.0);
  Eigen::Vector3d yaw3d_acc_end = Eigen::Vector3d (yaw_acc_end, 0.0, 0.0);

  vector<Eigen::Vector3d> yaw3d_start_end_derivative;
  yaw3d_start_end_derivative.push_back(yaw3d_rate_start);
  yaw3d_start_end_derivative.push_back(yaw3d_rate_end);
  yaw3d_start_end_derivative.push_back(yaw3d_acc_start);
  yaw3d_start_end_derivative.push_back(yaw3d_acc_end);

  Eigen::MatrixXd yaw3d_control_pts;
  NonUniformBspline::parameterizeToBspline(init_knot_span, yaw3dwpts, yaw3d_start_end_derivative, degree, yaw3d_control_pts);

  ROS_WARN_STREAM("The yaw3d control pts size is " << yaw3d_control_pts.rows());
  yaw_control_pts.clear();
  for(int i = 0; i < (int)yaw3d_control_pts.rows(); i++){
    double yaw_control_pt = yaw3d_control_pts(i, 0);
    yaw_control_pts.push_back(yaw_control_pt);
    std::cout << yaw_control_pt << " | ";
  }
  std::cout<<std::endl;
  ROS_WARN_STREAM("The yaw control pts size is " << yaw_control_pts.size());

  yaw_traj.setUniformBspline(yaw3d_control_pts, degree, init_knot_span);
  yaw_traj.setKnot(traj.getKnot());

  yaw_traj.computeDerivatives(1, yaw_derivative);
}



void printYawAndSave(){
  std::string PathFileName = ros::package::getPath("test_bspline") + "/data/yaw.txt";
  std::fstream f;
  f.open(PathFileName,std::ios::out);


  double duration = yaw_traj.getTimeSum();
  std::cout<<"the duration is : "<<duration<<std::endl;
  double time = 0.0;

  while (time < duration + 1e-4){
    Eigen::Vector3d yaw3d = yaw_traj.evaluateDeBoorT(time);
    Eigen::Vector3d yaw3d_rate = yaw_derivative[0].evaluateDeBoorT(time);
    //std::cout << "t: " << time << " |yaw: " << yaw3d[0]<< " |yaw3d rate: " << yaw3d_rate[0] << std::endl;

    f << time << ","
      << yaw3d(0) << "," << yaw3d_rate(0) << "\n";

    time = time + 0.1;
  }
  ROS_WARN_STREAM("Print and save yaw done.");
}

void calWithGivenYaw(){
  ROS_WARN_STREAM("Cal with given yaw");

  // cal the frist yaw to test
  double first_waypt_yaw = atan2(waypts_[1].y() - waypts_[0].y(), waypts_[1].x() - waypts_[0].x());
  double second_waypt_yaw = atan2(waypts_[2].y() - waypts_[1].y(), waypts_[2].x() - waypts_[1].x());
  std::cout << "The first waypt yaw is : " << first_waypt_yaw << std::endl;
  std::cout << "The second waypt yaw is : " << second_waypt_yaw << std::endl;

  double init_yaw = -2.0; // define by myself
  double max_yaw_change = init_knot_span * max_yaw_rate;
  yaw_waypts_.clear();
  yaw_waypts_.push_back(init_yaw);
  for(int i = 1; i < (int)waypts_.size()-1; i++){
    double next_yaw = atan2(waypts_[i+1].y() - waypts_[i].y(), waypts_[i+1].x() - waypts_[i].x());
    double delta_yaw = fabs(next_yaw - yaw_waypts_[i-1]);
    if(delta_yaw > max_yaw_change){
      next_yaw = yaw_waypts_[i-1] + fabs(delta_yaw) / delta_yaw * max_yaw_change;
    }
    yaw_waypts_.push_back(next_yaw);
  }
  yaw_waypts_.push_back(yaw_waypts_.back());
  ROS_WARN_STREAM("The yaw waypts size is : " << yaw_waypts_.size());

  // param to bspline
  vector<Eigen::Vector3d> yaw3dwpts;
  for(int i = 0; i < (int)yaw_waypts_.size(); i++){
    Eigen::Vector3d yaw3dwpt = Eigen::Vector3d (yaw_waypts_[i], 0.0, 0.0);
    yaw3dwpts.push_back(yaw3dwpt);
  }
  ROS_WARN_STREAM("The yaw 3d pts size is : "<<yaw3dwpts.size());

  Eigen::Vector3d yaw3d_rate_start = Eigen::Vector3d (yaw_rate_start, 0.0, 0.0);
  Eigen::Vector3d yaw3d_acc_start = Eigen::Vector3d (yaw_acc_start, 0.0, 0.0);
  Eigen::Vector3d yaw3d_rate_end = Eigen::Vector3d (yaw_rate_end, 0.0, 0.0);
  Eigen::Vector3d yaw3d_acc_end = Eigen::Vector3d (yaw_acc_end, 0.0, 0.0);

  vector<Eigen::Vector3d> yaw3d_start_end_derivative;
  yaw3d_start_end_derivative.push_back(yaw3d_rate_start);
  yaw3d_start_end_derivative.push_back(yaw3d_rate_end);
  yaw3d_start_end_derivative.push_back(yaw3d_acc_start);
  yaw3d_start_end_derivative.push_back(yaw3d_acc_end);

  Eigen::MatrixXd yaw3d_control_pts;
  NonUniformBspline::parameterizeToBspline(init_knot_span, yaw3dwpts, yaw3d_start_end_derivative, degree, yaw3d_control_pts);

  ROS_WARN_STREAM("The yaw3d control pts size is " << yaw3d_control_pts.rows());
  yaw_control_pts.clear();
  for(int i = 0; i < (int)yaw3d_control_pts.rows(); i++){
    double yaw_control_pt = yaw3d_control_pts(i, 0);
    yaw_control_pts.push_back(yaw_control_pt);
    std::cout << yaw_control_pt << " | ";
  }
  std::cout<<std::endl;
  ROS_WARN_STREAM("The yaw control pts size is " << yaw_control_pts.size());

  yaw_traj.setUniformBspline(yaw3d_control_pts, degree, init_knot_span);
  yaw_traj.setKnot(traj.getKnot());

  yaw_derivative.clear();
  yaw_traj.computeDerivatives(1, yaw_derivative);
}


void printGivenYawAndSave(){
  std::string PathFileName = ros::package::getPath("test_bspline") + "/data/yaw_given_start.txt";
  std::fstream f;
  f.open(PathFileName,std::ios::out);


  double duration = yaw_traj.getTimeSum();
  std::cout<<"the duration is : "<<duration<<std::endl;
  double time = 0.0;

  while (time < duration + 1e-4){
    Eigen::Vector3d yaw3d = yaw_traj.evaluateDeBoorT(time);
    Eigen::Vector3d yaw3d_rate = yaw_derivative[0].evaluateDeBoorT(time);
    //std::cout << "t: " << time << " |yaw: " << yaw3d[0]<< " |yaw3d rate: " << yaw3d_rate[0] << std::endl;

    f << time << ","
      << yaw3d(0) << "," << yaw3d_rate(0) << "\n";

    time = time + 0.1;
  }
  ROS_WARN_STREAM("Print and save yaw re done.");
}

void calYawToUniformBspline(){
  double duration = traj.getTimeSum();
  ROS_INFO_STREAM("The time sum is : " << duration);

//  double segment_num = 10.0;
//  double segment_time = duration / segment_time;

  vector<double> tmp_knot_vector;

  int segment_num = 10;
  yaw_dt = duration / 10.0;
  std::cout<<"the yaw dt is "<<yaw_dt<<std::endl;
  for (int i = 0; i <= segment_num; ++i) {
    double knot = duration * i / segment_num;
    tmp_knot_vector.push_back(knot);
  }

  yaw_waypts_.clear();
  double init_yaw = -2.0; // define by myself
  yaw_waypts_.push_back(init_yaw);
  double max_delta_yaw = max_yaw_rate * yaw_dt;
  for (int i = 1; i <= segment_num; i++){
    Eigen::Vector3d tmp_vel = derivative[0].evaluateDeBoorT(tmp_knot_vector[i]);
    double tmp_yaw = atan2(tmp_vel(1),tmp_vel(0));
    double delta_yaw = fabs(tmp_yaw - yaw_waypts_[i-1]);
    if(delta_yaw > max_delta_yaw){
      tmp_yaw = yaw_waypts_[i-1] + fabs(delta_yaw) / delta_yaw * max_delta_yaw;
      ROS_ERROR_STREAM("fast yaw change!");
    }
    std::cout<<tmp_yaw<<" |";
    yaw_waypts_.push_back(tmp_yaw);
  }
  std::cout<<std::endl;
  ROS_WARN_STREAM("The uniform yaw waypts is : " << yaw_waypts_.size());


  // param to bspline
  vector<Eigen::Vector3d> yaw3dwpts;
  for(double & yaw_waypt : yaw_waypts_){
    Eigen::Vector3d yaw3dwpt = Eigen::Vector3d (yaw_waypt, 0.0, 0.0);
    yaw3dwpts.push_back(yaw3dwpt);
  }
  ROS_WARN_STREAM("The yaw 3d pts size is : "<<yaw3dwpts.size());

  Eigen::Vector3d yaw3d_rate_start = Eigen::Vector3d (yaw_rate_start, 0.0, 0.0);
  Eigen::Vector3d yaw3d_acc_start = Eigen::Vector3d (yaw_acc_start, 0.0, 0.0);
  Eigen::Vector3d yaw3d_rate_end = Eigen::Vector3d (yaw_rate_end, 0.0, 0.0);
  Eigen::Vector3d yaw3d_acc_end = Eigen::Vector3d (yaw_acc_end, 0.0, 0.0);

  vector<Eigen::Vector3d> yaw3d_start_end_derivative;
  yaw3d_start_end_derivative.push_back(yaw3d_rate_start);
  yaw3d_start_end_derivative.push_back(yaw3d_rate_end);
  yaw3d_start_end_derivative.push_back(yaw3d_acc_start);
  yaw3d_start_end_derivative.push_back(yaw3d_acc_end);

  Eigen::MatrixXd yaw3d_control_pts;
  NonUniformBspline::parameterizeToBspline(yaw_dt, yaw3dwpts, yaw3d_start_end_derivative, degree, yaw3d_control_pts);

  ROS_WARN_STREAM("The yaw3d control pts size is " << yaw3d_control_pts.rows());
  yaw_control_pts.clear();
  for(int i = 0; i < (int)yaw3d_control_pts.rows(); i++){
    double yaw_control_pt = yaw3d_control_pts(i, 0);
    yaw_control_pts.push_back(yaw_control_pt);
    std::cout << yaw_control_pt << " | ";
  }
  std::cout<<std::endl;
  ROS_WARN_STREAM("The yaw control pts size is " << yaw_control_pts.size());

  yaw_traj.setUniformBspline(yaw3d_control_pts, degree, yaw_dt);
  yaw_derivative.clear();
  yaw_traj.computeDerivatives(1, yaw_derivative);
}

void saveUniformBsplineYaw(){
  std::string PathFileName = ros::package::getPath("test_bspline") + "/data/yaw_uniform.txt";
  std::fstream f;
  f.open(PathFileName,std::ios::out);


  double duration = yaw_traj.getTimeSum();
  std::cout<<"the duration is : "<<duration<<std::endl;
  double time = 0.0;

  while (time < duration + 1e-4){
    Eigen::Vector3d yaw3d = yaw_traj.evaluateDeBoorT(time);
    Eigen::Vector3d yaw3d_rate = yaw_derivative[0].evaluateDeBoorT(time);
    //std::cout << "t: " << time << " |yaw: " << yaw3d[0]<< " |yaw3d rate: " << yaw3d_rate[0] << std::endl;

    f << time << ","
      << yaw3d(0) << "," << yaw3d_rate(0) << "\n";

    time = time + 0.1;
  }
  ROS_WARN_STREAM("Print and save yaw uniform done.");
}



void getBspline(){
  bspline_.order = 3;
  bspline_.start_time = ros::Time::now();
  ROS_ERROR_STREAM("bspline_.start_time is " << bspline_.start_time);
  bspline_.traj_id = 0;
  bspline_.yaw_dt = yaw_dt;
  knot_vector = traj.getKnot();

  for (int i = 0; i < knot_vector.rows(); ++i) {
    bspline_.knots.push_back(knot_vector[i]);
  }
  for (int i = 0; i < yaw_control_pts.size(); i++){
    double yaw = yaw_control_pts[i];
    bspline_.yaw_pts.push_back(yaw);
  }

  for (int i = 0; i < (int)ctrl_pts_.rows(); i++){
    geometry_msgs::Point pt;
    pt.x = ctrl_pts_(i, 0);
    pt.y = ctrl_pts_(i, 1);
    pt.z = ctrl_pts_(i, 2);
    bspline_.pos_pts.push_back(pt);
  }
  have_bspline_ = true;
  ROS_WARN_STREAM("GET BSPLINE DONE");
}

int main(int argc, char** argv) {
  ros::init(argc, argv, "bspline_test");
  ros::NodeHandle nh("~");

  init(); // init the params
  load_init_path(); // init

  get_traj(); // get the bspline traj
  printPathAndSave(); // save the raw path data

  reallocate(); // time reallocate, bound the vel and acc
  printPathAndSaveAfterReallocate(); // save the path data after time reallocate

  calYaw(); // cal yaw for the traj
  printYawAndSave(); // print yaw pts and save

  calWithGivenYaw();
  printGivenYawAndSave();

  calYawToUniformBspline();
  saveUniformBsplineYaw();

  getBspline(); // get and pub bspline to the traj sever

  cpts_pub_ = nh.advertise<nav_msgs::Path>("/bspline/cpts",10);
  waypts_pub_ = nh.advertise<nav_msgs::Path>("/bspline/waypts",10);
  traj_pub_ = nh.advertise<nav_msgs::Path>("/bspline/traj",10);

  bspline_pub_ = nh.advertise<bspline::Bspline>("/planning/bspline", 10);

  ros::Timer display = nh.createTimer(ros::Duration(0.5), displayCallback);

  ros::spin();
  return 0;
}